#!/usr/bin/python
# -*- coding: UTF-8 -*-


import time,threading,sys
sys.path.append("/home/biowin/Minibot/04.RobotSamples/03.SpiderRobot")
from BwRobotLib import BwRobotLib
from SpiderRobot import SpiderRobot
from BwSensorLib import BwSensorLib
import os,inspect

# 加载机器人构型配置
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath     = os.path.dirname(os.path.abspath(fileName))
cfile=folderPath+"/config.txt"



class AvoidObsRobot:
	def __init__(self):
		# 实例化传感器库对象
		self.sensorlib = BwsensorliblibLib("udp")
		# 实例化BwRobotLib库
		self.robotlib = BwRobotLib("udp")
		# 实例化蜘蛛机器人库
		self.spider = SpiderRobot(self.robotlib)
		# 连接机器人设备
		self.robotlib.connectRobot("10.10.100.254",8899) #真机AP模式
        # 导入机器人配置参数
        self.robotlib.loadConfig(config_file)
		#配置ID
		self.spider.bindIds(self.robotlib.module_ids)
		# 时间变量
		self.wait_time = 0.2
		# 点阵显示
		# 点阵图案：向上箭头 向下箭头　向左箭头　向右箭头　
		self.display_data = [[0x36,0x49,0x49,0x00,0x3E],[0x41,0x36,0x00,0x1C,0x22]]	
	
	def avoidObsRobotMotion(self):
		while 1:
			dcm = self.sensor.readUltrasonicSensor() # 读取超声波传感器数据
			print(dcm)
			# 障碍物在前方0-15cm时，按照设计好的路径绕过障碍物，继续前进
			if (dcm <= 15 and dcm > 0):
				self.sensor.setMatrixSensor(self.display_data[1]) # 点阵显示开心表情
				# 左转
				for a in range(0,4):
					self.spider.turnLeft()
				# 前行
				for a in range(0,11):
					self.spider.goForward()
				# 转右
				for a in range(0,3):
					self.spider.turnRight()
				# 前行
				for a in range(0,8):
					self.spider.goForward()
				# 转右
				for a in range(0,2):
					self.spider.turnRight()
				# 前行
				for a in range(0,8):
					self.spider.goForward()
				# 转左
				for a in range(0,4):
					self.spider.turnLeft()
			else:
				self.sensor.setMatrixSensor(self.display_data[0]) # 点阵显示无语表情
				self.spider.goForward() # 前行
			
# 主函数			
def main():
    # 实例化避障机器人类
    avoidObsRobot = AvoidObsRobot()
    # 执行避障机器人功能
    avoidObsRobot.avoidObsRobotMotion()


if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        print(" Program closed! ")
    pass
